#include "data_management.h"


DataManagement::DataManagement(ros::NodeHandle *nodehandle) : nh_(*nodehandle)
{
}

DataManagement::~DataManagement()
{

}

void DataManagement::onIMUDataCallback(const sensor_msgs::Imu::ConstPtr &msg_in) 
{
    DataProviderIMUData::Instance()->Update(msg_in);
}

void DataManagement::onLidarPointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr &msg_in)
{
    DataProviderRawPointCloud::Instance()->Update(msg_in);
}

void DataManagement::init()
{
    aft_lidar_point_cloud_publisher_ = nh_.advertise<sensor_msgs::PointCloud2>("/livox/aft_lidar",10, true);
    lidar_objects_publisher_ = nh_.advertise<lidar_detect::lidar_objects>("/livox/lidar_3d", 10, true);
    imu_subscriber_ = nh_.subscribe("/livox/imu", 10000, &DataManagement::onIMUDataCallback, this);
    lidar_raw_point_subscriber_ = nh_.subscribe("/livox/lidar",100, &DataManagement::onLidarPointCloudCallback, this);    
}

void DataManagement::start()
{

}

bool DataProviderRawPointCloud::Update(const sensor_msgs::PointCloud2::ConstPtr &msg)
{
    std::lock_guard<std::mutex> lock(mutex_);
    pcl::PointCloud<pcl::PointXYZI> point_cloud;
    pcl::fromROSMsg(*msg, point_cloud);
    lspoint_cloud_queue_.Update(point_cloud);
    return true;
}

pcl::PointCloud<pcl::PointXYZI> const  DataProviderRawPointCloud::lspoint_cloud()
{
    pcl::PointCloud<pcl::PointXYZI> pl_orig;
    std::lock_guard<std::mutex> lock(mutex_);
    lspoint_cloud_queue_.Last(pl_orig);
    return pl_orig;
}

bool DataProviderIMUData::Update(const sensor_msgs::Imu::ConstPtr &msg)
{
    std::lock_guard<std::mutex> lock(mutex_);
    imu_data_queue_.Update(*msg);
    return true;
}

sensor_msgs::Imu const DataProviderIMUData::imu_data()
{
    sensor_msgs::Imu imu_data;
    std::lock_guard<std::mutex> lock(mutex_);
    imu_data_queue_.Last(imu_data);
    return imu_data;
}
